#include "PID.h"
#include "FIndLine.h"
#include "MOTOR.h"
#include "myOLED.h"
//470
#define SPEED   500
#define MAX     600
#define MIN     300

#define ERR1 15
#define ERR2 20
#define ERR3 30 //
#define ERR4 35//

//13.55
//
//-1.185
float kp = 13.55, ki = 0, kd = 1.085;//-1.190;

float err = 0;
float last_err = 0;
float P,I,D;
int HW_1 = 0,HW_2 = 0,HW_3 = 0,HW_4 = 0;


void stop(void);
void scan(void);//识别黑线
int red_pid(void);//计算pwm误差
void run(void);//控制电机
void scan(void)
{
	
	HW_4 = Find_Right2();
    HW_3 = Find_Right1();
    HW_2 = Find_Left1();
    HW_1 = Find_Left2();
	
	    if(HW_1 == 1 && HW_2 == 0 && HW_3 == 0 && HW_4 == 1)//1001  正常
		  {
				err = 0;
			}
			else if(HW_1 == 1 && HW_2 == 1 && HW_3 == 0 && HW_4 == 1)//1101 左
			{
				err = -ERR1;
			}
			else if(HW_1 == 1 && HW_2 == 1 && HW_3 == 1 && HW_4 == 0)//1110 左+
			{
				err = -ERR2;
			}
			else if(HW_1 == 1 && HW_2 == 1 && HW_3 == 0 && HW_4 == 0)//1100 左++
			{
				err = -ERR3;
			}
			else if(HW_1 == 1 && HW_2 == 0 && HW_3 == 0 && HW_4 == 0)//1000 左+++
			{
				err = -ERR4;
			}
			else if(HW_1 == 1 && HW_2 == 0 && HW_3 == 1 && HW_4 == 1)//1011 右
			{
				err = ERR1;
			}
			else if(HW_1 == 0 && HW_2 == 1 && HW_3 == 1 && HW_4 == 1)//0111 右+
			{
				err = ERR2;
			}
			else if(HW_1 == 0 && HW_2 == 0 && HW_3 == 1 && HW_4 == 1)//0011 右++
			{
				err = ERR3;
			}
			else if(HW_1 == 0 && HW_2 == 0 && HW_3 == 0 && HW_4 == 1)//0001 右+++
			{
				err = ERR4;
			}
			else if(HW_1 == 1 && HW_2 == 1 && HW_3 == 1 && HW_4 == 1)//1111
			{
				//motorPidSetSpeed(SPEED,SPEED);
			}
}
 
int red_pid(void)
{
	int change = 0;
	P = err;
	I += err;
	D = err - last_err;
	if(err == 0)
		I = 0;
	
	change = P*kp + I*ki +D*kd;
	last_err = err;
	
	return change;
}

void run(void)
{
	int pwm1,pwm2;
	int ch_val = red_pid();
	pwm2 = SPEED + ch_val;
	pwm1 = SPEED - ch_val;
    // if(pwm2>MAX)        pwm2=MAX;
    // else if(pwm2<MIN)   pwm2=MIN;
    // if(pwm1>MAX)        pwm1=MAX;
    // else if(pwm1<MIN)   pwm1=MIN;
	motorPidSetSpeed(pwm1,pwm2);
}
void stop(void)
{
	motorPidSetSpeed(0,0);//给参数电机参数的
}

void Show_HW()
{
    MyOLED_ShowNum(3, 1, HW_1, 1);
    MyOLED_ShowNum(3, 3, HW_2, 1);
    MyOLED_ShowNum(3, 5, HW_3, 1);
    MyOLED_ShowNum(3, 7, HW_4, 1);
}
